#pragma once

#include "joy_stick_advanced.hpp"


using namespace UNITREE_LEGGED_SDK;

class stateMachine
{
private:
    int state;

public:
    stateMachine()
    {
        state = 0;
    }

    int getState()
    {
        return state;
    }

    bool updateState(joyStickAdvanced& jsa)
    {
        if (jsa.joy_stick.keys.L2 % 2 == 1 && jsa.joy_stick.keys.B % 2 == 1)
        {
            state = -2;
            return true;
        }

        switch (state)
        {
            case -2:
                if (jsa.joy_stick.keys.R2 == 3)
                {
                    state += 1;
                    return true;
                }
                break;
            case -1:
                if (jsa.joy_stick.keys.R2 == 3)
                {
                    state += 1;
                    return true;
                }
                if (jsa.joy_stick.keys.R1 == 3)
                {
                    state -= 1;
                    return true;
                }
                break;
            case 0:
                if (jsa.joy_stick.keys.start == 3)
                {
                    state += 1;
                    return true;
                }
                if (jsa.joy_stick.keys.R1 == 3)
                {
                    state -= 1;
                    return true;
                }
                break;
            case 1:
                if (jsa.joy_stick.keys.R1 == 3)
                {
                    state -= 1;
                    return true;
                }
                break;
            case 2:
                if (jsa.joy_stick.keys.start == 3)
                {
                    state += 1;
                    return true;
                }
                if (jsa.joy_stick.keys.R1 == 3)
                {
                    state = 0;
                    return true;
                }
                break;
            case 3:
                if (jsa.joy_stick.keys.R1 == 3)
                {
                    state = 0;
                    return true;
                }
                break;

            default:
                return false;
        }

        return false;
    }

    void setState(int s)
    {
        state = s;
    }
};